import random
import math
from collections import deque
from pprint import pprint
from math import pi
com_municate = 0 # just some globals for communication, these ALWAYS start with com_
com_goto_flag = [0,0,0]
MAX_goto_flag = []#[4,2,2]
#self.goalList = [(500,400),(220,220),(220,580)]
com_team = ''
com_ammo_nav = {} # ammo points which will be taken by teammate
com_enemy_nav = {} # enemies which will be shot by teammate
directions = []
MAX_SPEED = 40
MAX_SHOT_DISTANCE  = 60
round = 0 # number of rounds
first = 0
com_agents = [] # (ammo, controlpointCoords, camping (0 = on way, 1 = camping, 2 = leave spot, 3 = no camping))
#com_agents[observation['id'] - 1]
com_controlPoints = [0, 0, 0] #0 = neutral, 1 = ours, 2 is ours + defending (=sitting on it)
# 3 is ours + defending with ammo
#-1 is taken by opponent, -2 is taken and defended on spot, -3 + ammo
com_controlAgentAtP = [0, 0, 0] # wether one of our agent was at this point last round
com_locationsVisited = [[70,70, -15], [70, 730, -15], [730, 70, -15], [730, 730, -15], [730, 490, -15], [ 730, 310, -15], [200, 330, -15], [200, 470, -15], [90, 400, -15]] # points to visit to keep track of 
# what our agents 'recently' have seen; x, y, score (nr of round where this point was last seen).


class AgentBrain():
    def __init__(self):
        """Put any initialization code here."""
        #Create navigational instance
        self.nav = Navigate()
        self.team = ''
        self.prevP = Point(-1,-1,0,False)
                
    def action(self, observation):
        #pprint(observation)
        global round, first, MAX_goto_flag, directions, com_agents, com_team, com_controlPoints
        global com_ammo_nav, com_enemy_nav, com_controlAgentAtP
        
        self.team = observation['team']
        self.nav.id = observation['id']
        
        #A lot of observation dependent things that need to happen once at start of game
        if first == 0:
            com_team = observation['team']
            first = observation['id']
            MAX_goto_flag = [5,3,3]
            for i in range(8):
                com_agents.append([0,(-1,-1),0])
            #if observation['team'] == 'Red':
            #    MAX_goto_flag = [4,2,3]
            #elif observation['team'] == 'Blue':
            #    MAX_goto_flag = [4,3,2]
            #else:
            #    print "LOLWUT: We are color",observation['team']
            
        com_agents[observation['id'] - 1][0] = observation['ammo'] # set the ammo so teammates know about it
            
        if observation['id'] == first:
            #print '\t %i' % (round) #First active agent this round' #reset some variables here?!
            com_ammo_nav = {}
            com_enemy_nav = {}
            directions = []
            round = round + 1
            #'controlpoints': [{'location': (500, 400), 'team': 'Neutral'},
            #print com_controlPoints
            for i in range(3):
                if com_controlPoints[i] <= 0 and com_team == observation['controlpoints'][i]['team']:
                    com_controlPoints[i] = 1 # setting it to 2 or 3 happens when that agent checks it
                    #print 'now owning control '+str(i) # we took the point last turn
                elif com_controlPoints[i] >= 0 and com_team != observation['controlpoints'][i]['team'] \
                        and 'Neutral' != observation['controlpoints'][i]['team']:
                    com_controlPoints[i] = -1 # opponent took the point last turn
                    #print 'we lost control '+str(i)
                    # if we saw an enemy at this point last time, we assume it's still their (why -> so we only 
                    #  go to this point when we got ammo)
                elif com_controlPoints[i] > 1 and com_controlAgentAtP[i] == 0:
                    # if no agents where near this one last round, set it back to 1
                    com_controlPoints[i] = 1
                    
            com_controlAgentAtP = [0, 0, 0]
        loc = 'location'
        (x,y) = observation[loc]

        #Create point for current location
        cp = Point(x,y,0,False)

        id = observation['id']

        #Reset path if just killed
        if observation['respawn']:
            #print '%i omfg we got killed at %s' % (round, str(self.prevP.coord))
            self.nav.setPath(x,y,observation)
        elif cp.getDist(self.prevP) > MAX_SPEED + 4:
            #print '%i --Moved a lot from %s : %d, resetting path..(killed but not properly sent by server)'
            #% (round, str(self.prevP.coord), cp.getDist(self.prevP))
            self.nav.setPath(x,y,observation)
        elif self.nav.goalInt >= 0 and com_controlPoints[self.nav.goalInt] < -1 and observation['ammo'] == 0:
            #print '%i .. taking a new target; previous %i is heavily guarded! %i %i' % (id, self.nav.goalInt, x, y)
            self.nav.setPath(x,y,observation)
        elif self.nav.goalInt >= 0 and com_agents[id-1][2] != 1 and ((com_controlPoints[self.nav.goalInt] == 3 and observation['ammo'] < 5) \
                or (com_controlPoints[self.nav.goalInt] == 2 and observation['ammo'] < 3)):
            #print '%i .. taking a new target; previous %i is defended already! %i %i' % (id, self.nav.goalInt, x, y)
            self.nav.setPath(x,y,observation)
        elif len(self.nav.path) <= 2 and self.nav.camping != 1 and \
                cp.getDist(self.nav.currentGoal) < 2.5:
            #print '%i .. Goal reached and not camping -> pick a new goal %i %i' % (id, x, y)
            self.nav.setPath(x,y,observation)
        elif self.nav.camping == 0 and observation['ammo'] > 0:
            #print '%i .. we got ammo, time to start camping/fighting! %i %i' % (id, x, y)
            self.nav.setPath(x,y,observation)
        elif com_agents[id-1][2] == 2:
            #print '%i: someone else likes this spot better.. time to move! %i %i' % (id, x, y)
            self.nav.setPath(x,y,observation)
        
        self.controlPoints(cp,id) # check if we should take over some control point to camp there
        

        #Store in action format

        self.prevP = cp #
        
        # if we are close to a control point (ie we can see it)
        # then check all enemies to see if they are defending it
        # (only if com_team != observation['controlpoints'][i]['team'] )

        self.nav.preNav(cp, self.team, observation, round)
        
        #check for enemies to 'take'
        (sh,t,s) = self.nav.shoot(cp, self.team, observation)
        
        if sh == 0: # we are not shooting (and not preparing to shoot)
            #check for ammo to take + add to path if going for
            self.nav.getAmmo(cp, observation)
             
            #Get speed and turn based on current path
            (s,t) = self.nav.getMove(observation[loc][0], observation[loc][1], \
                observation['direction'])
            if round < 5 and abs(t) > pi/3:
                s = 20
                
        elif sh == 1:
            com_agents[observation['id'] - 1][0] = observation['ammo'] - 1 # more accurate update of ammo
        else: # gonna shoot in 2-turns?
            sh = 0
        
        """#WORK IN PROGRESS#
        #Absolute angle
        a = t + observation['direction']
        #Normal angle
        a = math.pi - a
        
        #Start and end points of trajectory line
        sl = (x + math.cos(a-math.pi)*(3), y + math.sin(a-math.pi)*(3))
        el = (x + math.cos(a)*(s+3), y + math.sin(a)*(s+3))
        
        #Monster equation that makes 2 lines describing the agents path
        lines = [(((sl[0] + math.cos(a-0.5*math.pi)*(3),sl[1] + \
        math.sin(a-0.5*math.pi)*(3)), (el[0] + math.cos(a-0.5*math.pi)*(3), \
        el[1] + math.sin(a-0.5*math.pi)*(3)))), ((sl[0] + \
        math.cos(a+0.5*math.pi)*(3),sl[1] + math.sin(a+0.5*math.pi)*(3)), \
        (el[0] + math.cos(a+0.5*math.pi)*(3),el[1] + math.sin(a+0.5*math.pi)* \
        (3)))]
        
        fl = False
        for d in directions:
            if self.nav.intersect(d[0],d[1],lines[0][0],lines[0][1]) \
                or self.nav.intersect(d[0],d[1],lines[1][0],lines[1][1]):
                fl = True
                break
        if fl:
            s = 0
        else:
            directions.extend(lines)
        """#END WORK IN PROGRESS#
        
        action = {
            'turn': t,
            'speed': s,
            'shoot': sh
        }
        return action
    
    # check if we should control some controlpoint or to leave
    def controlPoints(self,cPoint,id):
        global com_agents
        
        cVal = com_agents[id-1]
        if cVal[2] != 1:
            gPoint = Point(cVal[1][0],cVal[1][1],0,False)
            if cPoint.getDist(gPoint) <= MAX_SPEED * 2:
                camp = []                                      
                for i in com_agents:
                    if i[1] == cVal[1] and i[2] == 1: # if controlpoint is the same and it's camping
                        camp = i
                        break
                if camp == []:
                    cVal[2] = 1 # no other campers, we now go camping
                elif cVal[0] > camp[0]: # we got more ammo
                    cVal[2] = 1 # we go camp
                    camp[2] = 2 # other camper has to move away
                    #print '----We %s stay, moving: %s' % (str(cVal), str(camp))
                else:
                    cVal[2] = 2# we go move
                    #print '----WE %s MOVE' % str(cVal)
    
        
class Navigate:
    def __init__(self):
        #List regions > List points > Tuple x,y,cross
        self.initList =  [ [ (650,160,1)] , \
            [ (650,640,2) ], \
            [ (265,750,3),(50,535,4) ], \
            [ (50,465,4),(50,335,5),(265,400,7) ], \
            [ (50,265,5),(265,50,6) ], \
            [ (565,160,1),(565,640,2),(335,750,3),(335,400,7),(335,50,6) ] ]
        #Control-points: first one is currently preferred, then 2nd if blue
        self.goalList = [(500,400),(220,220),(220,580)]
        #Using initList, creation connectivity graph
        self.constructGraph()
        #Current list of waypoints in path
        self.path = deque([])
        #Current history (length == 2) of previous points
        self.hist = deque([])
        self.goalInt = -1 # -1 if we don't have a goal from the goalList
        self.currentGoal = ''
        self.id = 0
        self.camping = 0 # camping.. or not

    def preNav(self,cp,team,observation, round):
        # some pre Navigation steps..
        # go over all visible agents to see if any of them occupy enemy controlpoint
        # only a controlpoint which we can see to be checked (current map it's max 1 same time)
        global com_controlPoints, com_locationsVisited, com_controlAgentAtP, com_agents
        id = observation['id']
        cpId = -1
        gP = '' # the goalPoint we can see (if there is one)
        oldCPState = 0
        if com_agents[id-1][2] == 1: # we were camping; reset
            com_agents[id-1][2] = 0
        for i in range(3): # for all controlpoints
            #com_controlPoints
            controlP = self.goalList[i]
            gPt = Point(controlP[0], controlP[1],0,False)
            d = cp.getDist(gPt) 
            if d <= 100:
                cpId = i
                gP = Point(controlP[0], controlP[1],0,False)
                
                if com_controlPoints[cpId] == 0:
                    self.path.appendleft(gP) # neutral point close by.. let's go!
            
                elif d <= 95:   # the 2.5 is so that we can see an enemy even if it's at other side of point
                    
                    if com_controlPoints[cpId] == -2: # last state it was theirs + occupied
                        com_controlPoints[cpId] = -1 # set it to just 'theirs' and see if it's occupied next
                        oldCPState = -2
                    if d <= 5: # we are standing on the controlpoint
                        com_controlAgentAtP[cpId] = 1
                        if com_controlPoints[cpId] > 0: #standing on it AND it's ours
                            #print '%i standing on %i %i' % (id, controlP[0], controlP[1])
                            if cpId == self.goalInt: # reached our goal
                                #print '%i reached goal %i %i' % (id, controlP[0], controlP[1])
                                self.camping = 1
                                if com_agents[id-1][2] != 2: # aren't running so should go camping
                                    com_agents[id-1][2] = 1
                            if observation['ammo'] > 0:
                                com_controlPoints[cpId] = 3
                            else:
                                com_controlPoints[cpId] = 2
                    
        for agent in observation['agents']:
            if agent['team'] != team: # for now we only care about the opponents
                #if str(agent['location']) in com_enemy_nav: %i enemy %s getting shot!
                ep = Point(agent['location'][0],agent['location'][1],0,False)
                
                if cpId >= 0 and com_controlPoints[cpId] == -1: # check if an enemy is on this spot
                    d = gP.getDist(ep)
                    if d <= 5:
                        com_controlPoints[cpId] = -2 # taken + defended
                #TODO: check in how many turns we can kill this enemy
                # and put the enemy in a prio list to shoot
        if cpId >= 0 and com_controlPoints[cpId] == -1:
            #print 'this point is not defended.. let\'s just plan to move through it asap!'
            controlP = self.goalList[cpId]
            tp = Point(controlP[0], controlP[1],0,False)
            self.path.appendleft(tp)
        for locations in com_locationsVisited:
            [x, y, score] = locations
            ep = Point(x, y,0,False)
            if cp.getDist(ep) < 20: # TODO, get a more logical check here?
                locations[2] = round # currently checks if it's next to this point but point
                #print 'we are at %i %i (for ammo checking) new score: %i' % (x, y, round)
                # where it should go to and point where it checks should perhaps be different
        return
                        
    def shoot(self,cp,team,observation):
        global com_enemy_navm, com_agents
        
        if observation['ammo'] == 0:
            return (0,0,0) #grr we don't got ammo to shoot :/
        sa = 0
        sd = 0
        sh = 0
        for agent in observation['agents']:
            if agent['team'] != team:
                if str(agent['location']) in com_enemy_nav:
                    #print '%i enemy %s getting shot!' % (round, str(agent['location']))
                    pass
                else:
                    ep = Point(agent['location'][0],agent['location'][1],0,False)
                    # TODO: add line interference check instead of region for shooting
                    if ep.region == cp.region:
                        #Get distance and angle
                        d = cp.getDist(ep)
                        a = cp.getAngle(ep)
                        #print '\t dist: %s ang: %s' % (d, a)
                        #movesReq = 0
                        #Recalc angle based on current direction
                        a -= observation['direction']
                        if abs(a) > math.pi:
                            if a >= 0:
                                a -= 2*math.pi
                            else:
                                a += 2*math.pi
                                
                        #print 'recalc angle: '+str(a)
                        #If angle is greater than maximum turn, means you can't reach this turn
                        #movesReq += int(math.ceil(abs(a) / (math.pi / 3))) - 1
                        #movesReq += int(math.ceil(d / MAX_SHOT_DISTANCE)) - 1
                        #if movesReq < 1:
                            #print '%i Shoot enemy at %s' % (round, str(agent['location']))
                            #(s,t) = (d,a) # use this dist/angle instead of current
                          
                            #let other agents known this enemy is ours!
                        sh = 2
                        if abs(a) <= math.pi / 3 and (int(math.ceil(d / MAX_SHOT_DISTANCE)) - 1) <= 0:
                            com_enemy_nav[str(agent['location'])] = observation['id']
                            sh = 1
                            return (sh, a, 0) # no need to check other enemies
                        if com_agents[observation['id']-1][2] == 1: # we're camping and on spot
                            sd = 0
                        elif (int(math.ceil(d / MAX_SHOT_DISTANCE)) - 1) <= 0:
                            sd = 0
                        else:
                            sd = d - (MAX_SHOT_DISTANCE - 1)
                            
                        sa = a
                        
              #          else:
              #              print 'Shoot NOT: too far'
              #      else:
              #          print 'Shoot NOT: tother region, not going!'   

            
            #print "av_agent: %i , location: %s , direct: %f  ,colour: %s " % (av_agent['id'],
            #av_agent['location'], av_agent['direction'], av_agent['team'])
            # kinda same comments as with the ammo
        if sh == 2:
            #print '%i 2-step targetting! %i camping %i x/y: %s ' % (observation['id'], sd, com_agents[observation['id']-1][2], str(observation['location']))
            return (sh, sa, sd) # targetting a 2 step target               
        return (0, 0, 0) #aw, nothing to shoot :/

    def getAmmo(self, cp, observation):
        global com_ammo_nav
      
        bestmovesReq = 9999
        bestAp = ''
        bestAmmo = ''
        for ammo_av in observation['ammopacks']:
            #print "ammo_av location: %i %i " % (ammo_av['location'][0], ammo_av['location'][1])
            #when to take ammo?
            #-rank available ammo according to easyest to take? (probably overkill)
            #-always take if without ammo?
            #-always take if easy to take? (ie getting not much further from destination)
            #-never take if it requires more then 2 movements in 'wrong' direction?
            if str(ammo_av['location']) in com_ammo_nav:
                pass #print '\tGRR ammo getting picked by teammate'
            else:    
                #create point for this ammo pack
                #ammo_reg = self.nav.getRegion(ammo_av['location'][0],ammo_av['location'][1])
                #print 'ammo region: '+str(ammo_reg)
                ap = Point(ammo_av['location'][0],ammo_av['location'][1],0,False)
                
                if ap.region == cp.region:
                    #Get distance and angle
                    d = cp.getDist(ap)
                    a = cp.getAngle(ap)
                    #print '\t dist: %s ang: %s' % (d, a)
                    movesReq = 0
                    #Recalc angle based on current direction
                    a -= observation['direction']
                    if abs(a) > math.pi:
                        if a >= 0:
                            a -= 2*math.pi
                        else:
                            a += 2*math.pi
                            
                    #print 'recalc angle: '+str(a)
                    #Calculate required moves
                    movesReq += (int(math.ceil(abs(a) / (math.pi / 3))) - 1)
                    #If not near goal, do not rotation gets more weight
                    if (len(self.path) > 1):
                        movesReq *= 2
                    movesReq += int(math.ceil(d / MAX_SPEED)) - 1
                    if movesReq < bestmovesReq:
                        bestmovesReq = movesReq
                        bestAp = ap
                        bestAmmo = ammo_av
               #     else:
               #         print 'ammo too far'
               # else:
               #     print 'other region, not going!'   
        if bestmovesReq <= math.floor(12/(observation['ammo'] + 1)):
            #print 'getting ammo!'
            #(s,t) = (d,a) # use this dist/angle instead of current
            self.path.appendleft(bestAp)
            #let other agents known this ammo is ours!
            com_ammo_nav[str(bestAmmo['location'])] = observation['id']
            return # no need to check other ammo points               
            
    def constructGraph(self):
        #Create region list
        self.regions = []
        #For each region
        for r in self.initList:
            reg = []
            #For each points
            for p in r:
                reg.append(Point(p[0],p[1],p[2],True))
            #Connect all points in the same region
            for p1 in reg:
                for p2 in reg:
                    p1.addNeigh(p2)
            #Add region to region list
            self.regions.append(reg)
        #Connect all points in the same crossing
        #C = Cross counter, starting at crossID 1
        c = 1
        while True:
            #Store all the points of that crossID C
            t = []
            #For each region
            for r in self.regions:
                for p in r:
                    if p.cross == c:
                        t.append(p)
            #If there are no points of crossID C
            if len(t) == 0:
                break
            #Connect all points in the same crossing
            for p1 in t:
                for p2 in t:
                    p1.addNeigh(p2)
            #Check the next crossing
            c += 1
    
    def setPath(self,x,y,obs):
        global com_agents, com_locationsVisited, com_goto_flag, com_controlPoints
        #Reset history
        self.hist = deque([])
        com_agents[self.id - 1][2] = 0 # we are on way to next camp spot
        
        distr_flag = [MAX_goto_flag[0] - com_goto_flag[0], \
                        MAX_goto_flag[1] - com_goto_flag[1], \
                        MAX_goto_flag[2] - com_goto_flag[2]]
        
        if self.goalInt > -1: # decrease previous goto goal
            distr_flag[self.goalInt] = 0
            com_goto_flag[self.goalInt] = com_goto_flag[self.goalInt] - 1
            if com_goto_flag[self.goalInt] < 0: # I don't think this ever occurs..
                com_goto_flag[self.goalInt] = 0
        #random 'distribution' of the goals

        for i in range (3):
            # check if we currently should not go to some goal:
            # A: don't go if it's taken by opponent and defended unless you got ammo yourself
            if com_controlPoints[i] < -1 and obs['ammo'] == 0:
                distr_flag[i] = 0
            # B: don't go if it's (defended by us with ammo)? TODO: check for 'enough' ammo at controlp?
            elif com_controlPoints[i] == 3:
                distr_flag[i] = 0
            elif -2 < com_controlPoints[i] and com_controlPoints[i] < 2: #theirs/neutral/ours but NOT defended
                distr_flag[i] = distr_flag[i] + 4
        # how many places where?
        
        gp = ''
        randomP = random.randint(0,sum(distr_flag)+ min(10, obs['ammo']) + 1) # without ammo higher chance for random
        if (distr_flag[0] == 0 and distr_flag[1] == 0 and distr_flag[2] == 0) or \
                randomP == 0:
          #  if randomP != 0:
          #      print '--- We got no valid goals to pick from! .. Go find a corner and cry? :/ '
          #  else:
          #      print '---going to visit some "random" point..'
                            
            com_agents[self.id - 1][2] = 3 # going to some random spot for ammo.. don't camp there..
            self.camping = 0
            i = 0
            lowestID = 0
            lowestScore = com_locationsVisited[0][2] # score of first location
            lowest2ndScore = com_locationsVisited[0][2]
            for locations in com_locationsVisited:
                [lx, ly, score] = locations
                if score < lowest2ndScore: 
                    if score < lowestScore:
                        lowest2ndScore = lowestScore
                        lowestScore = score
                        lowestID = i
                    else:
                        lowest2ndScore = score
                i += 1
            lx = com_locationsVisited[lowestID][0]
            ly = com_locationsVisited[lowestID][1]
            gp = Point(lx,ly,0,False) #
           # print '--going to %i %i for ammo (location score %i)' % (lx, ly, com_locationsVisited[lowestID][2])
            com_locationsVisited[lowestID][2] = com_locationsVisited[lowestID][2]  + 5
            if com_locationsVisited[lowestID][2] <= lowest2ndScore: # to prevent all agents going to a really low point
                com_locationsVisited[lowestID][2] = lowest2ndScore + 5 
            #print '\tnew score: %i' %  com_locationsVisited[lowestID][2]    
            com_agents[self.id - 1][1] = (lx, ly) # set goal coordinates    
            self.goalInt = -1
        else:
            self.camping = 1 # it's allowed to camp at goalList spots
            rdist = random.randint(0,max(sum(distr_flag), 1))
            r = 0
            if rdist < distr_flag[0]:
                r = 0
            elif rdist < distr_flag[0] + distr_flag[1]:
                r = 1
            else:
                r = 2
            g = self.goalList[r]
            com_agents[self.id - 1][1] = g # set the controlpointCoords
            
            com_goto_flag[r] = com_goto_flag[r] + 1 # increase number of agents going to this spot
            self.goalInt = r
            #com_goto_flag = [0,0,0] start values
            #MAX_goto_flag = [5,3,3]

            gp = Point(g[0],g[1],0,False)
        
        self.currentGoal = gp
        #Creat point for current loc
        #cr = self.getRegion(x,y)
        cp = Point(x,y,0,False)
        #Connect current point with others in region
        for p in self.regions[cp.region]:
            cp.addNeigh(p)
        #Queue of breadth-first paths and their total distance
        #Current path distance is 0 and consists only of current point
        q = deque([[0,[cp]]])
        #Minimal distance and path to goal
        minDist = -1
        minPath = []
        #While still elements in queue
        while len(q) > 0:
            #Pop path from queue
            p = q.popleft()
            #If the last waypoint in the path is in the goal region
            if p[1][-1].region == gp.region:
                #Distance from last waypoint to goal
                d = p[1][-1].getDist(gp)
                #If total distance is minimal
                if minDist == -1 or p[0] + d < minDist:
                    #Add the goal the path
                    np = p[1][:]
                    np.append(gp)
                    #Store path as current minimum
                    minDist = p[0] + d
                    minPath = np
            #Else: Keep expanding paths
            else:
                #For each point that is connected to the last waypoint
                #in the path
                for i in p[1][-1].neigh:
                    #If the new point is not yet in the path
                    if i[0] not in p:
                        #Add new point to path
                        np = p[1][:]
                        np.append(i[0])
                        #Increase the total distance
                        nd = p[0]+i[1]
                        #If current distance is smaller than minimum
                        if minDist == -1 or nd < minDist:
                            #Append the path to the end of the queue
                            q.append([nd,np])
        self.path = deque(minPath)
    

    def getMove(self,x,y,dir):
        global com_agents
        #Create point for current location
        cp = Point(x,y,0,False)
        
        
        #Determine total movement w.r.t. history
        dt = 0
        #For each point in the history
        for i in self.hist:
            #Increase the total distance
            dt += cp.getDist(i)
        #Add current point to history
        self.hist.append(cp)
        #If length of history > 2
        if len(self.hist) > 2:
            #Pop 1 element from history
            self.hist.popleft()
        
        
        if dt < 1 and len(self.hist) == 2 and com_agents[self.id - 1][2] != 1: #If total movement < 1: 
                # Stuck, make random turn (to the right only!)
            return (15,random.random()*pi*2/9 + pi/9) #at least pi/9 turn, max pi/3
        else:
            #Init distance and angle
            d = 0
            a = dir
            while len(self.path) > 0:
                #If there are no more waypoints left
                #if len(self.path) == 0:
                    #Get new path
                #    self.setPath(x,y)
                #Pop waypoint from the current path
                wp = self.path.popleft()
                #Get distance and angle
                d = cp.getDist(wp)
                a = cp.getAngle(wp)
                #If remaining distance > 4; meaning you have not reached
                #the waypoint yet
                if d > 5 or len(self.path) == 0:
                    #Reappend waypoint to front of path
                    self.path.appendleft(wp)
                    break
                
            # we are at at out goalpoint and we were planning to camp; let's camp!
            if com_agents[self.id - 1][2] == 1 and d < 2: #TODO set back to == 1
                #set correct angle for this spot
                d = 0
                a = self.getPrefDirection(com_agents[self.id - 1][1])
      #      else:
            #Recalc angle based on current direction
            a -= dir
            if abs(a) > math.pi:
                if a >= 0:
                    a -= 2*math.pi
                else:
                    a += 2*math.pi
            #If angle is greater than maximum turn, do not move forward
            if abs(a) > math.pi / 3:
                d = 5
 
            #Return distance,angle
            return (d,a)
    def getPrefDirection(self, xy):
        global com_team
        (x,y) = xy
        if x == 220 and y == 580:
            return 0.75*math.pi
        elif x == 220 and y == 220:
            return -0.75*math.pi
            
        if com_team == 'Red':
                        
            return -0.5*math.pi
        else:
            return 0.5*math.pi

    def intersect(self,a1,a2,b1,b2):
        p3 = ((a1[0] - a2[0])*(b1[1] - b2[1]) - (a1[1] - a2[1])*(b1[0] - b2[0]))
        if (p3 != 0):
            p1 = (a1[0]*a2[1] - a1[1]*a2[0])
            p2 = (b1[0]*b2[1] - b1[1]*b2[0])
            x = (p1*(b1[0] - b2[0]) - (a1[0] - a2[0])*p2) / p3
            if min(a1[0],a2[0]) <= x and x <= max(a1[0],a2[0]):
                y = (p1*(b1[1] - b2[1]) - (a1[1] - a2[1])*p2) / p3
                return True
        return False

class Point:
    def __init__(self,x,y,cross,way):
        #Random offset for waypoints, but not for control-points
        if way:
            x += random.randint(-30,30)
            y += random.randint(-30,30)
        #Store point variables
        self.coord = (x,y)
        self.cross = cross
        self.region = self.getRegion(x, y)
        self.neigh = []
        self.check = []
        self.waypoint = way
    
    def addNeigh(self,n):
        #Check if it is not the same point or already connected
        if n != self and n not in self.check:
            #Compute distance between points
            d = self.getDist(n)
            #Store as point,distance tuple
            self.neigh.append((n,d))
            #Store as point
            self.check.append(n)
    
    def getDist(self,n):
        #Compute the distance between the points
        x = n.coord[0]-self.coord[0]
        y = n.coord[1]-self.coord[1]
        d = math.sqrt(x**2+y**2)
        return d
    
    def getAngle(self,n):
        #Compute the angle between points
        x = n.coord[0]-self.coord[0]
        y = n.coord[1]-self.coord[1]
        a = -math.atan2(-y,x)
        return a
    #Get the region, pretty hard-coded
    def getRegion(self,x,y):
        if x > 798 or x < 2 or y > 798 or y < 2:
            return -1 # to prevent going to ammo spawnpoints on walls
        if x >= 600:
            if y < 400:
                return 0 # region A
            return 1 # region B
        if x >= 300:
            return 5 # region F
        if y >= 500:
            return 2 # region C
        if y >= 300:
            return 3 # region D
        return 4 # region E
